
#include "mavlink_interface.h"
#include "fastmavlink_functions.h"

uint8_t serial_has_space(mavlink_interface_t *mav)
{
    return (__HAL_UART_GET_FLAG(mav->mavuart, UART_FLAG_TXE)) ? 1 : 0;
}

void serial_write_buf(mavlink_interface_t *mav, uint16_t len)
{
    HAL_UART_Transmit_DMA(mav->mavuart, mav->tx_buf, len);
}

uint8_t sendFeedBackstatus(mavlink_interface_t *mav, ip2366_state_t *ip2366, system_state_t *sys)
{
    mav->fdb_info.error_code = 0;
    if (sys->sys_state_next == SYS_OFF) mav->fdb_info.error_code |= 1;
    if (ip2366->error_state == 2) mav->fdb_info.error_code |= (1 << 1);
    if (ip2366->error_state == 4) mav->fdb_info.error_code |= (1 << 2);
    if (ip2366->fault_bitmask == 8) mav->fdb_info.error_code |= (1 << 3);
    if (ip2366->fault_bitmask == 16 || ip2366->fault_bitmask == 32) mav->fdb_info.error_code |= (1 << 4);
    if (ip2366->current_battery < 100 && ip2366->current_battery > -100) mav->fdb_info.error_code |= (1 << 5);

    uint16_t count = fmav_msg_pl_fdb_info_pack_to_frame_buf(
    mav->tx_buf,
    PL_ID_SLAVE,
    mav->my_compid,
    ip2366->voltages[0],
    ip2366->current_battery,
    ip2366->mode,
    mav->fdb_info.error_code,
    mav->fdb_info.power_key_state,
    mav->fdb_info.debug_key_state,
    &mav->status
    );
    if (serial_has_space(mav))
    {
        // fmav_msg_to_frame_buf(mav->tx_buf, &mav->msg);
        serial_write_buf(mav, count);
        return 1; // we have successfully send it, so tell that
    }

    return 0;
}

// our message handler
__weak void handleMessage(mavlink_interface_t *mav, fmav_message_t *msg)
{
}

// The uart data handler
void MavlinkData(mavlink_interface_t *mav, uint8_t c)
{
    uint8_t res = fmav_parse_to_frame_buf(&mav->result, mav->rx_buf, &mav->status, c);

    if (res == FASTMAVLINK_PARSE_RESULT_OK)
    {
        // 有脏东西
        // const fmav_message_entry_t *msg_entry = fmav_get_message_entry(FASTMAVLINK_MSG_ID_BATTERY_STATUS);
        res = fmav_check_frame_buf(&mav->result, mav->rx_buf);

        if (res == FASTMAVLINK_PARSE_RESULT_OK)
        {
            fmav_frame_buf_to_msg(&mav->msg, &mav->result, mav->rx_buf);
            handleMessage(mav, &mav->msg);
        }
    }
}

// this is a example whitch should be called repeatedly
// void spinOnce(void)
// {
//     // let's first check and do incoming messages

//     uint16_t available = serial_available();
//     for (uint16_t i = 0; i < available; i++)
//     {
//         uint8_t c;
//         serial_read_char(&c);

//         MavlinkData(c);
//     }

//     // let's now send a STATUSTEXT message, if it has been triggered

//     if (send_statustext)
//     {
//         // if (sendStatustext())
//         // {
//         //         }
//         send_statustext = 0; // we have successfully send it, so clear that flag
//     }

//     // let's now send a HEARTBEAT message, to tell the world we exist

//     uint32_t tnow_ms = get_time_ms();
//     if ((tnow_ms - tlast_ms) > 1000)
//     {
//         if (sendHeartbeat())
//         {
//             tlast_ms += 1000; // we have successfully send it, so do it again later
//             // LED_TOGGLE;
//         }
//     }
// }

//------------------------------
// Default Arduino setup() and loop() functions
//------------------------------

// void loop()
// {
//     spinOnce();
// }
